#include <iostream>
#include <string>
#include <fstream>
#include <thread>
#include <deque>
#include <yaml-cpp/yaml.h>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <GeographicLib/LocalCartesian.hpp>

class savePath2TUM
{
public:
    std::string root_dir = ROOT_DIR;

    std::string SLAM_Path_Topic;
    std::string SLAM_Path_Path;
    std::string saveFile_SLAMPath;

    int reflash_File_SLAMPath;

    ros::NodeHandle nh;
    ros::Subscriber subSLAMPath;

    std::ofstream Path2TUMFiles;

    savePath2TUM()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/savePath2TUMConfig.yaml");
        SLAM_Path_Topic = yaml["SLAM_Path_Topic"].as<std::string>();    // 查询config的配置文件
        SLAM_Path_Path  = yaml["SLAM_Path_Path"].as<std::string>();     // 查询config的配置文件
        saveFile_SLAMPath = std::string(std::string(ROOT_DIR)) + SLAM_Path_Path;

        reflash_File_SLAMPath = system((std::string("exec rm ") + saveFile_SLAMPath).c_str());


        subSLAMPath = nh.subscribe<nav_msgs::Path>(SLAM_Path_Topic, 1000, &savePath2TUM::SLAMPath_handle, this, ros::TransportHints().tcpNoDelay());
    }


    void SLAMPath_handle(const nav_msgs::Path::ConstPtr& msgIn)
    {
        nav_msgs::Path data = *msgIn;
        geometry_msgs::PoseStamped last_pose_stamped = data.poses.back();
        // geometry_msgs::Pose last_pose = last_pose_stamped.pose;

        double time, x, y, z, qx, qy, qz, qw;
        time = last_pose_stamped.header.stamp.toSec();
        x = last_pose_stamped.pose.position.x;
        y = last_pose_stamped.pose.position.y;
        z = last_pose_stamped.pose.position.z;

        qx = last_pose_stamped.pose.orientation.x;
        qy = last_pose_stamped.pose.orientation.y;
        qz = last_pose_stamped.pose.orientation.z;
        qw = last_pose_stamped.pose.orientation.w;

        saveData2TUM(saveFile_SLAMPath, time, x, y, z, qx, qy, qz, qw);
    }


    /************************************************************
     * @brief 将数据写入成TUM的轨迹数据格式文件
     * 
     * @param file 文件路径
     * @param time 时间戳
     * @param x 
     * @param y 
     * @param z 
     * @param qx 
     * @param qy 
     * @param qz 
     * @param qw 
     ***********************************************************/
    void saveData2TUM(std::string file, double time, double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        Path2TUMFiles.open(file, std::ios::app);
        if (Path2TUMFiles.is_open())
        {
            Path2TUMFiles << std::fixed << time
                << " " << x
                << " " << y
                << " " << z
                << " " << qx
                << " " << qy
                << " " << qz
                << " " << qw
                << std::endl;
            Path2TUMFiles.close();
        }
    }


    void savePathProcess()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }    
    }

};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "savePath2TUM");

    savePath2TUM MO;

    ROS_INFO("\033[1;32m----> savePath2TUM Procee Started.\033[0m");

    std::thread savePath2TUMThread(&savePath2TUM::savePathProcess, &MO);

    ros::spin();

    savePath2TUMThread.join();

    return 0;
}